#include <nav_msgs/Odometry.h>
#include <ros/ros.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>

class LaserTfToOdomPublisher {
public:
    LaserTfToOdomPublisher()
        : tf_listener_(tf_buffer_)
    {
        ros::NodeHandle private_nh("~");

        private_nh.param<std::string>("odom_frame", odom_frame_, "odom");
        private_nh.param<std::string>("base_frame", base_frame_, "base_link");

        odom_pub_ = nh_.advertise<nav_msgs::Odometry>("/laser_odom", 10);
    }

    void spin()
    {
        ros::Rate rate(50); // 50Hz
        while (ros::ok()) {
            publishOdomFromTF();
            rate.sleep();
        }
    }

private:
    void publishOdomFromTF()
    {
        geometry_msgs::TransformStamped transform_stamped;

        try {
            transform_stamped = tf_buffer_.lookupTransform(
                odom_frame_, base_frame_, ros::Time(0), ros::Duration(0.1));

            nav_msgs::Odometry odom_msg;
            odom_msg.header.stamp = transform_stamped.header.stamp;
            odom_msg.header.frame_id = odom_frame_;
            odom_msg.child_frame_id = base_frame_;

            odom_msg.pose.pose.position.x = transform_stamped.transform.translation.x;
            odom_msg.pose.pose.position.y = transform_stamped.transform.translation.y;
            odom_msg.pose.pose.position.z = transform_stamped.transform.translation.z;
            odom_msg.pose.pose.orientation = transform_stamped.transform.rotation;

            // 不填速度，robot_localization 会自己估算
            odom_pub_.publish(odom_msg);
        } catch (tf2::TransformException& ex) {
            ROS_WARN_THROTTLE(1.0, "TF lookup failed: %s", ex.what());
        }
    }

    ros::NodeHandle nh_;
    ros::Publisher odom_pub_;
    tf2_ros::Buffer tf_buffer_;
    tf2_ros::TransformListener tf_listener_;

    std::string odom_frame_;
    std::string base_frame_;
};

int main(int argc, char** argv)
{
    ros::init(argc, argv, "laser_tf_to_odom_publisher");
    LaserTfToOdomPublisher node;
    node.spin();
    return 0;
}
